package levels;

import java.util.LinkedList;

import math.Vector2D;

/**
 * 
 * @author Ryan Hill
 *
 */
public class PathFinder
{
	private PathGrid map;
	private LinkedList<Node> openCells;
	private LinkedList<Node> closedCells;
	
	public PathFinder( PathGrid pf )
	{
		map = pf;
		openCells = new LinkedList<Node>();
		closedCells = new LinkedList<Node>();
	}
	
	/**
	 * Find a path from the location "start" to the location "end" 
	 * using the A* path finding algorithm.
	 * 
	 * Description of A*: http://en.wikipedia.org/wiki/A*_search_algorithm
	 * Code below partially derived from psuedo code written in the above
	 * web page.
	 * 
	 * @param start The start of the path
	 * @param end The end of the path
	 * @return The path from start to end
	 */
	public LinkedList<Vector2D> findPath( Vector2D start, Vector2D end )
	{
		Node t1 = map.getContainingTile( start );
		Node t2 = map.getContainingTile( end );
		
		LinkedList<Vector2D> path = findPath( t1, t2 );
		
		if( path != null )	
		{
			path.addLast( end );
		}
		
		return path;
	}
	
	/**
	 * Find a path from the tile "start" to the tile "end" 
	 * using the A* path finding algorithm.
	 * 
	 * @param start The first tile in the path
	 * @param end The last tile in the path
	 * @return The path from tile "start" to tile "end"
	 */
	private LinkedList<Vector2D> findPath( Node start, Node end )
	{
		if( start.isOccupied() || end.isOccupied() )
			return null;
		
		openCells.add( start );
		Node current = start;
		
		double min = -1;
		Node minTile = null;
		
		while( openCells.size() > 0 )
		{
			for( Node t : openCells )
			{
				if( !t.isOccupied() && !closedCells.contains( t ) )
				{
					if( !openCells.contains( t ) )
						openCells.add( t );
					
					if( t.getF() < min || min == -1 )
					{
						min = t.getF();
						minTile = t;
					}
				}
			}
			
			current = minTile;
			
			if( current.equals( end ) )
				break;
			
			openCells.remove( current );
			closedCells.add( current );
			
			for( Node t : map.getAdjacentTiles( current ) )
			{
				if( !t.isOccupied() && !closedCells.contains( t ) )
				{
					double tentativeG = current.getG() + g( /* current, t */ );
					boolean isBetter = false;
					
					if( !openCells.contains( t ) )
					{
						t.setExpandedFrom( current );
						t.setG( g( /* current, t */ ) );
						t.setH( h( t.getPosition(), end.getPosition() ) );
						openCells.add( t );
						
						isBetter = true;
					}
					else if( tentativeG < t.getG() )
					{
						isBetter = true;
					}
					
					if( isBetter )
					{
						t.setExpandedFrom( current );
						t.setG( tentativeG );
					}
				}
			}
			
			min = -1;
			minTile = null;
		}
		
		openCells.clear();
		closedCells.clear();
		
		LinkedList<Vector2D> path = new LinkedList<Vector2D>();
		
		Node pc = end;
		while( ( pc != null ) && ( pc != start ) )
		{
			path.addFirst( pc.getPosition() );
			pc = pc.getExpandedFrom();
		}
		
		for( int i = 0; i < map.getTileHeight(); i++ )
		{
			for( int j = 0; j < map.getTileWidth(); j++ )
			{
				map.getTiles().get( i ).get( j ).setExpandedFrom( null );
			}
		}
		
		return path;
	}

	/**
	 * Estimated distance (very close) from one tile to the next.
	 * Value used in f(x) = g(x) + h(x) as part of the A* algorithm
	 * 
	 * @param from The tile traveling from
	 * @param to The tile traveling to
	 * @return The estimated cost of traveling from "from" to "to"
	 */
	private int g( /* Node from, Node to */ )
	{
		/* For diagonal tiles only
		int angle = PathGrid.getAngle( from.getPosition(), to.getPosition() );
		
		if( ( angle / 45 ) % 2 == 1 )
			return 14;
		*/
		return 10;
	}
	
	/**
	 * The heuristic value for A* path finding algorithm.
	 * 
	 * @param g1 The location of the tile whose heuristic is being calculated
	 * @param end The location of the last tile in the path
	 * @return An estimated distance from g1 to end
	 */
	private double h( Vector2D g1, Vector2D end )
	{
		double xDistance = Math.abs( g1.getX() - end.getX() );
		double yDistance = Math.abs( g1.getY() - end.getY() );
		
		return 10 * ( yDistance + xDistance );
	}

}
